#ifndef BAG_DEALER_DATAEXTRACTOR_HPP
#define BAG_DEALER_DATAEXTRACTOR_HPP

#include <rosbag/bag.h>
#include <rosbag/view.h>
#include <sensor_msgs/Image.h>
#include <sensor_msgs/PointCloud2.h>
#include <message_filters/synchronizer.h>
#include <message_filters/subscriber.h>
#include <message_filters/sync_policies/approximate_time.h>
#include <cv_bridge/cv_bridge.h>
#include <pcl/io/pcd_io.h>
#include <pcl_conversions/pcl_conversions.h>
#include <boost/filesystem.hpp>
#include <boost/foreach.hpp>
#include <opencv2/opencv.hpp>
#include <string>
#include <vector>
#include <openssl/md5.h>
#include <bag_dealer/Sample.h>


namespace BagDealer {
    // virtual base class of data extractor
    class ExtractorBase {
    public:
        ExtractorBase() = delete;
        ExtractorBase(const std::string& bagName,
                      const std::string& packageName,
                      const std::string& destName) :
        bagName(bagName), packageName(packageName), destName(destName)
        {
            bagNameNoSuffix = unbag(bagName);
            std::string packagePath = ros::package::getPath(packageName);
            if (packagePath.empty())
            {
                ROS_ERROR_STREAM("Package does not exist!");
                exit(1);
            }
            else
            {
                datasetPath = boost::filesystem::path(packagePath) / "nuscenes" / destName;
                if (!boost::filesystem::exists(datasetPath.string()))
                {
                    boost::filesystem::create_directories(datasetPath);
                    ROS_INFO_STREAM("Create dataset base path at" << datasetPath.string());
                }

                ROS_INFO_STREAM("Dataset base path already exist at" << datasetPath.string());

                bagPath = boost::filesystem::path(packagePath) / "bags" / bagName;
                if (!boost::filesystem::exists(bagPath))
                {
                    ROS_ERROR_STREAM("BAG " << bagName << " DOES NOT EXIST!");
                    exit(1);
                }
                else
                {
                    try
                    {
                        bag.open(bagPath.string(), rosbag::bagmode::Read);
                    }
                    catch (rosbag::BagException& e)
                    {
                        ROS_ERROR_STREAM("LOADING BAG FAILED " << e.what());
                        exit(1);
                    }
                    ROS_INFO_STREAM("Bag " << bagName << " opened.");
                }
            }
        }

        ~ExtractorBase() = default;
        virtual void extract() = 0;
        std::string timestampConverter(const std_msgs::Header& header);
        std::string computeMD5(const std::string& input);
        bool is_first = true;


    protected:
        std::string bagName;
        std::string packageName;
        std::string destName;
        std::string bagNameNoSuffix;
        rosbag::Bag bag;
        ros::NodeHandle nh_;
        boost::filesystem::path datasetPath;
        boost::filesystem::path bagPath;
        inline std::string unbag(const std::string& input)
        {
            size_t dotLoc = input.find_last_of('.');
            return input.substr(0, dotLoc);
        }
        ros::Time keyFrameTime;
        ros::Time getTimeFromStamp(const std_msgs::Header& ts);
//        DS::SampleList serializer;

    };

    class SweepsExtractor : public ExtractorBase
    {
    public:
        SweepsExtractor(const std::string& bagName,
                       const std::string& packageName,
                       const std::string& destName) :
        ExtractorBase(bagName, packageName, destName)
        {
            sweepsPath = datasetPath / "sweeps";
            boost::filesystem::create_directories(sweepsPath);
        }

        ~SweepsExtractor() = default;
        void extract() override;


    private:
        boost::filesystem::path sweepsPath;
        void extractVelodyne();
        void extractCam(const std::string& camName, const std::string& topicName);
    };


    class SamplesExtractor : public ExtractorBase
    {
    public:
        SamplesExtractor(const std::string& bagName,
                         const std::string& packageName,
                         const std::string& destName) :
                ExtractorBase(bagName, packageName, destName)
        {
            samplesPath = datasetPath / "samples";
            boost::filesystem::create_directories(samplesPath);
        }

        ~SamplesExtractor();
        void extract() override;

    private:
        boost::filesystem::path samplesPath;
        void initSynchronizers();
        void callback(const sensor_msgs::ImageConstPtr& frontImgMsg,
                      const sensor_msgs::ImageConstPtr& leftImgMsg,
                      const sensor_msgs::ImageConstPtr& rightImgMsg,
                      const sensor_msgs::PointCloud2ConstPtr& pcMsg);
        bool extractVelodyne(const sensor_msgs::PointCloud2ConstPtr& pcMsgPtr);
        void extractCam(const std::string& camName, const sensor_msgs::ImageConstPtr& imgMsgPtr);
    };

}


#endif //BAG_DEALER_DATAEXTRACTOR_HPP
